classdef HelperLoopFilter < matlab.System
    % HelperLoopFilter 用于载波跟踪环路（PLL/FLL）和码跟踪环路（DLL）的环路滤波器
    % 支持1阶、2阶、3阶环路配置，可选择矩形积分器或双线性积分器
    % 设计参考：Kaplan《Understanding GPS/GNSS》第8章环路滤波器设计
    
    properties
        noiseBandwidth  % 环路噪声带宽（Hz），可动态调整
    end

    properties(Nontunable)
        order = 1       % 环路阶数（1、2、3），不可动态修改
        flagBoxCarInteg = false  % 是否使用矩形积分器（否则用双线性积分器）
    end

    properties(Access = private)
        % 环路参数
        naturalFrequency  % 自然频率（与噪声带宽相关）
        gain1, gain2, gain3  % 各阶环路增益
        
        % 积分器状态（用于维持滤波连续性）
        x1 = 0  % 一阶积分器状态
        x2 = 0  % 二阶积分器状态（仅3阶环路使用）
    end

    methods(Access = public)
        function obj = HelperLoopFilter(noiseBandwidth, order, flagBoxCarInteg)
            % 构造函数：初始化环路基本参数
            % 输入：
            %   noiseBandwidth - 噪声带宽（Hz）
            %   order - 环路阶数（1/2/3）
            %   flagBoxCarInteg - 是否使用矩形积分器
            obj.noiseBandwidth = noiseBandwidth;
            obj.order = order;
            obj.flagBoxCarInteg = flagBoxCarInteg;
        end

        function gain = getGain(obj)
            % 获取当前环路增益（用于调试或分析）
            switch obj.order
                case 1
                    gain = obj.gain1;
                case 2
                    gain = [obj.gain2; obj.gain1];
                case 3
                    gain = [obj.gain3; obj.gain2; obj.gain1];
                otherwise
                    gain = [];
            end
        end
    end

    methods(Access = protected)
        function setupImpl(obj)
            % 初始化时计算环路参数（增益、自然频率）
            obj.calcLoopParams();
        end

        function delta = stepImpl(obj, err, steptime)
            % 处理单步误差信号，输出环路调整量
            % 输入：
            %   err - 鉴别器输出的误差信号（相位误差或频率误差）
            %   steptime - 积分时间（秒），即两次滤波的时间间隔
            % 输出：
            %   delta - 环路调整量（相位或频率的修正值）
            
            loopfilterout = 0;  % 滤波器输出
            
            switch obj.order
                case 3  % 3阶环路：双积分器结构
                    % 第一级积分器（双线性或矩形）
                    x2New = err * obj.gain1 * steptime + obj.x2;  % 比例-积分环节
                    if obj.flagBoxCarInteg
                        integ1out = x2New + err * obj.gain2;  % 矩形积分
                    else
                        integ1out = (x2New + obj.x2) / 2 + err * obj.gain2;  % 双线性积分（梯形近似）
                    end
                    obj.x2 = x2New;  % 更新积分器状态
                    
                    % 第二级积分器
                    x1New = integ1out * steptime + obj.x1;
                    if obj.flagBoxCarInteg
                        loopfilterout = x1New + err * obj.gain3;
                    else
                        loopfilterout = (x1New + obj.x1) / 2 + err * obj.gain3;
                    end
                    obj.x1 = x1New;  % 更新积分器状态
                    
                case 2  % 2阶环路：单积分器结构
                    x1New = err * obj.gain1 * steptime + obj.x1;
                    if obj.flagBoxCarInteg
                        loopfilterout = x1New + err * obj.gain2;
                    else
                        loopfilterout = (x1New + obj.x1) / 2 + err * obj.gain2;
                    end
                    obj.x1 = x1New;
                    
                case 1  % 1阶环路：无积分器，仅比例环节
                    loopfilterout = err * obj.gain1;
            end
            
            % 计算最终调整量（乘以时间步长，将频率误差转为相位调整，或频率调整量）
            delta = steptime * loopfilterout;
        end

        function resetImpl(obj)
            % 重置积分器状态（如跟踪中断后重启）
            obj.x1 = 0;
            obj.x2 = 0;
        end

        function processTunedPropertiesImpl(obj)
            % 当可调参数（如噪声带宽）变化时，重新计算环路参数
            obj.calcLoopParams();
        end
    end

    methods(Access = private)
        function calcLoopParams(obj)
            % 计算环路参数（自然频率和各阶增益）
            % 参数取值参考Kaplan《Understanding GPS/GNSS》第8章表8.23
            switch obj.order
                case 1
                    % 1阶环路：自然频率 = 噪声带宽 / 0.25
                    obj.naturalFrequency = obj.noiseBandwidth / 0.25;
                    obj.gain1 = obj.naturalFrequency;  % 比例增益
                    
                case 2
                    % 2阶环路：阻尼系数~0.707（sqrt(2)/2≈1.414）
                    obj.naturalFrequency = obj.noiseBandwidth / 0.53;
                    obj.gain1 = obj.naturalFrequency ^ 2;  % 积分增益
                    obj.gain2 = obj.naturalFrequency * 1.414;  % 比例增益
                    
                case 3
                    % 3阶环路：增加对频率变化率的跟踪
                    obj.naturalFrequency = obj.noiseBandwidth / 0.7845;
                    obj.gain1 = obj.naturalFrequency ^ 3;  % 二阶积分增益
                    obj.gain2 = (obj.naturalFrequency ^ 2) * 1.1;  % 一阶积分增益
                    obj.gain3 = obj.naturalFrequency * 2.4;  % 比例增益
                    
                otherwise
                    error('不支持的环路阶数，仅支持1、2、3阶');
            end
        end
    end
end